/*
 * Motor.c
 *
 *  Created on: 2025锟斤拷9锟斤拷12锟斤拷
 *      Author: Move
 */


#include "zf_common_headfile.h"
#include <math.h>




// 舵机参数变量定义
float Servo_Middle = 637 ;
float Servo_Max = 715;
float Servo_Min = 559;

int16 ENCO_Wish_L;
int16 ENCO_Wish_R;
int16 Basic_Speed;
PID_T Speed_L;
PID_T Speed_R;
PID_T Angle;

// PID参数初始化函数
void PID_Init(void)
{
    // 左电机速度PID
    Speed_L.Kp = 5;
    Speed_L.Ki = 0;
    Speed_L.Kd = 0;
    Speed_L.OutMax = 8000;
    Speed_L.OutMin = -8000;
    Speed_L.MAXInt = 0;
    
    // 右电机速度PID
    Speed_R.Kp = 5;     // 和左边一样
    Speed_R.Ki = 0;
    Speed_R.Kd = 0;
    Speed_R.ErrorInt = 0;
    Speed_R.OutMax = 8000;
    Speed_R.OutMin = -8000;
    Speed_R.MAXInt = 0;
    

    // 舵机角度PID参数初始化
    Angle.ErrorInt = 0;
    Angle.MAXInt = 0;
    Angle.Error0 = 0;
    Angle.Error1 = 0;
    Angle.Out = 0;
    Angle.OutMax = 78;
    Angle.OutMin = -78;
}


//舵机初始化
void Servo_Init(void)
{
    pwm_init(Servo_Pin,50,Servo_Middle);
}

float Servo_Pwm ;
void Servo_Control(){

    float error=0;
    static float last_error=0;

    error = (float)ImageStatus.Det_True;
    Angle.Kp  = fuzzy_p;
    Angle.kp1 = fuzzy_p1;
    Angle.Kd  = fuzzy_d;
//
//    Angle.Kp = Fuzzy_P(error, (error-last_error));
//    Angle.Kd = Fuzzy_D(error, (error-last_error));
    Angle.Target = 0;                                   //0->使行驶中线与中线值误差为0
    Angle.Actual = (float)ImageStatus.Det_True;         //ImageStatus.Det_True->当前中线值误差
    PosPId_Updata(&Angle);
    Servo_Pwm =  Servo_Middle - Angle.Out;   //输出等于打角值+舵机中位值

    last_error = error;  // 更新历史误差
    
    //控制限幅，保护舵机
    if(Servo_Pwm> Servo_Max){
        Servo_Pwm = Servo_Max;
    }
    else if(Servo_Pwm < Servo_Min){
        Servo_Pwm = Servo_Min;
    }
    //输出pwm
    pwm_set_duty(Servo_Pin, Servo_Pwm);
    last_error = error;

}


//电机初始化
void Motor_Init(void){
    //左电机
    pwm_init(Motor_L_Pwm, 17000, 0);
    gpio_init(Motor_L_Dir, GPO, 1, GPO_PUSH_PULL);
    //右电机
    pwm_init(Motor_R_Pwm, 17000, 0);
    gpio_init(Motor_R_Dir, GPO, 1, GPO_PUSH_PULL);
}

void Motor_PWM(int Left_Pwm , int Right_Pwm){
    if(Left_Pwm >= 0){                          //左电机
        gpio_set_level(Motor_L_Dir,1);          //0->正转
        pwm_set_duty(Motor_L_Pwm, Left_Pwm);
    }
    else{
        gpio_set_level(Motor_L_Dir, 0);
        pwm_set_duty(Motor_L_Pwm, -Left_Pwm);
    }

    if(Right_Pwm >= 0){                         //右电机
        gpio_set_level(Motor_R_Dir, 1);         //0->正转
        pwm_set_duty(Motor_R_Pwm, Right_Pwm);
    }
    else{
        gpio_set_level(Motor_R_Dir, 0);        //1->反转
        pwm_set_duty(Motor_R_Pwm, -Right_Pwm);
    }
}





//差速计算 - 阿克曼转向
// 车辆参数：轴距L=0.2m, 轮距W=0.155m, 最大转角38°

void speed_diff(void){
    double pwm_range = Servo_Max - Servo_Min;
    double angle_range = 76;
    double angle_deg = (Servo_Pwm - Servo_Middle) / pwm_range * angle_range;
    
    // 转换为弧度
    double angle_rad = angle_deg * 3.14159265358979323846 / 180.0;
    
    // 计算转弯半径
    double R = 20 / tan(angle_rad);
    
    // 计算角速度和差速
    double radian = Basic_Speed / R;

    int16 diff_value = (int)(radian * 7.75);

    if(diff_value > 0)
    {
        ENCO_Wish_L = Basic_Speed - diff_value;
        ENCO_Wish_R = Basic_Speed;
    }
    else
    {
        ENCO_Wish_L = Basic_Speed;
        ENCO_Wish_R = Basic_Speed + diff_value;
    }
//    ENCO_Wish_L = Basic_Speed - diff_value;
//    ENCO_Wish_R = Basic_Speed + diff_value;

    // 限制速度范围
    if (ENCO_Wish_L < -8000) ENCO_Wish_L = -8000;
    if (ENCO_Wish_L > 8000) ENCO_Wish_L = 8000;
    if (ENCO_Wish_R < -8000) ENCO_Wish_R = -8000;
    if (ENCO_Wish_R > 8000) ENCO_Wish_R = 8000;
}

void Motor_PID(void){

    Speed_L.Target = ENCO_Wish_L;
    Speed_R.Target = ENCO_Wish_R ;

    Speed_L.Actual = encoder_L;
    InsPId_Updata(&Speed_L);

    Speed_R.Actual = encoder_R;
    InsPId_Updata(&Speed_R);

    Motor_PWM(Speed_L.Out, Speed_R.Out);
}
